use core::{
    ffi::c_void,
    marker::PhantomData,
    ptr::{addr_of_mut, null_mut},
    usize,
};

use crate::{
    c::{
        osMessageQueueAttr_t, osMessageQueueGet, osMessageQueueId_t, osMessageQueueNew,
        osMessageQueuePut, osMessageQueueReset, osRtxMessageQueue_t, osWaitForever,
    },
    error::{parse_empty, parse_val, Error},
    utils::Align8Store,
};

const CB_SIZE: usize = size_of::<osRtxMessageQueue_t>();

/**
 *
 *
 * /// Memory size in bytes for Memory Pool storage.
/// \param         block_count   maximum number of memory blocks in memory pool.
/// \param         block_size    memory block size in bytes.
#define osRtxMemoryPoolMemSize(block_count, block_size) \
  (4*(block_count)*(((block_size)+3)/4))

/// Memory size in bytes for Message Queue storage.
/// \param         msg_count     maximum number of messages in queue.
/// \param         msg_size      maximum message size in bytes.
#define osRtxMessageQueueMemSize(msg_count, msg_size) \
  (4*(msg_count)*(3+(((msg_size)+3)/4)))


 */

#[inline(always)]
pub const fn mq_size<T>(count: u32) -> usize {
    4 * (count as usize) * (3 + ((size_of::<T>() + 3) / 4))
}

pub struct MessageQueue<T, const MQ_SIZE: usize, const MSG_COUNT: u32, const MSG_SIZE: u32> {
    _p: PhantomData<T>,
    cb_mem: Align8Store<CB_SIZE>,
    mq_mem: Align8Store<MQ_SIZE>,
    id: osMessageQueueId_t,
}

impl<T: Default, const MQ_SIZE: usize, const MSG_COUNT: u32, const MSG_SIZE: u32>
    MessageQueue<T, MQ_SIZE, MSG_COUNT, MSG_SIZE>
{
    pub const fn default() -> Self {
        Self {
            _p: PhantomData,
            cb_mem: Align8Store::new(),
            mq_mem: Align8Store::new(),
            id: null_mut(),
        }
    }

    pub fn new(&mut self, name: &str) {
        let attr = osMessageQueueAttr_t {
            name: name.as_ptr() as *const i8,
            attr_bits: 0,
            cb_mem: self.cb_mem.raw_ptr(),
            cb_size: CB_SIZE as u32,
            mq_mem: self.mq_mem.raw_ptr(),
            mq_size: MQ_SIZE as u32,
        };
        self.id = unsafe { osMessageQueueNew(MSG_COUNT, MSG_SIZE, &attr) };
        assert!(self.id != null_mut(), "mq new fail");
    }

    pub fn post(&self, msg: &T) -> Result<(), Error> {
        let r = unsafe { osMessageQueuePut(self.id, msg as *const T as *const c_void, 0, 0) };
        parse_empty(r)
    }

    pub fn take(&self) -> T {
        let mut t = T::default();
        unsafe {
            osMessageQueueGet(
                self.id,
                addr_of_mut!(t) as *mut c_void,
                null_mut(),
                osWaitForever,
            )
        };
        t
    }

    pub fn poll(&self, timeout: u32) -> Result<T, Error> {
        let mut t = T::default();
        let r = unsafe {
            osMessageQueueGet(self.id, addr_of_mut!(t) as *mut c_void, null_mut(), timeout)
        };
        parse_val(r, t)
    }

    pub fn reset(&self) {
        unsafe { osMessageQueueReset(self.id) };
    }
}
